/*-----------------------------------------------------------------------------

 Copyright 2017 Hopsan Group

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.


 The full license is available in the file LICENSE.
 For details about the 'Hopsan Group' or information about Authors and
 Contributors see the HOPSANGROUP and AUTHORS files that are located in
 the Hopsan source code root directory.

-----------------------------------------------------------------------------*/

#ifndef AEROVEHICLETVC2_HPP_INCLUDED
#define AEROVEHICLETVC2_HPP_INCLUDED

#include <iostream>
#include "ComponentEssentials.h"
#include "ComponentUtilities.h"
#include "math.h"

//!
//! @file AeroVehicleTVC2.hpp
//! @author Petter Krus <petter.krus@liu.se>
//! @date Sun 2 Jun 2013 19:44:11
//! @brief Flight dynamics model of aircraft
//! @ingroup AeroComponents
//!
//==This code has been autogenerated using Compgen==
//from 
/*{, C:, HopsanTrunk, HOPSAN++, CompgenModels}/AeroVehicleTVC2.nb*/

using namespace hopsan;

class AeroVehicleTVC2 : public ComponentC
{
private:
     double Cd0b;
     double Ctvc;
     double CLalphabh;
     double CLalphabv;
     double hthrust0;
     double Ix;
     double Ixz;
     double Iy;
     double Iz;
     double Me;
     double Sbh;
     double Sbv;
     double xbach;
     double xbacv;
     double xbcge;
     double xcargo;
     double xfuel;
     double xeng;
     double yeng;
     double kground;
     double cground;
     Port *mpPtvcly;
     Port *mpPtvclz;
     Port *mpPtvcry;
     Port *mpPtvcrz;
     double delayParts1[9];
     double delayParts2[9];
     double delayParts3[9];
     double delayParts4[9];
     double delayParts5[9];
     double delayParts6[9];
     double delayParts7[9];
     double delayParts8[9];
     double delayParts9[9];
     double delayParts10[9];
     double delayParts11[9];
     double delayParts12[9];
     double delayParts13[9];
     Matrix jacobianMatrix;
     Vec systemEquations;
     Matrix delayedPart;
     int i;
     int iter;
     int mNoiter;
     double jsyseqnweight[4];
     int order[13];
     int mNstep;
     //Port Ptvcly variable
     double tortvcly;
     double thetatvcly;
     double wtvcly;
     double ctvcly;
     double Zctvcly;
     double eqInertiatvcly;
     //Port Ptvclz variable
     double tortvclz;
     double thetatvclz;
     double wtvclz;
     double ctvclz;
     double Zctvclz;
     double eqInertiatvclz;
     //Port Ptvcry variable
     double tortvcry;
     double thetatvcry;
     double wtvcry;
     double ctvcry;
     double Zctvcry;
     double eqInertiatvcry;
     //Port Ptvcrz variable
     double tortvcrz;
     double thetatvcrz;
     double wtvcrz;
     double ctvcrz;
     double Zctvcrz;
     double eqInertiatvcrz;
//==This code has been autogenerated using Compgen==
     //inputVariables
     double thrustl;
     double thrustr;
     double g0;
     double Mfuel;
     double Mcargo;
     double rho;
     double vturbx;
     double vturby;
     double vturbz;
     double wturbx;
     double wturby;
     double wturbz;
     //outputVariables
     double xcg;
     double ycg;
     double zcg;
     double vx;
     double vy;
     double vz;
     double Psi;
     double Thetao;
     double Phi;
     double Ub;
     double Vb;
     double Wb;
     double Pb;
     double Qb;
     double Rb;
     double q0;
     double q1;
     double q2;
     double q3;
     double AlphaAttack;
     double BetaSlip;
     double altitude;
     double gfx;
     double gfy;
     double gfz;
     double CL1;
     double Cd1;
     double Fax;
     double Faz;
     //InitialExpressions variables
     //LocalExpressions variables
     double v;
     double Alpha;
     double qpress;
     double Beta;
     double mass;
     double xbcg;
     double Dragb;
     double Mdvtheta;
     double Mdvpsi;
     double Fx;
     double Fy;
     double Fz;
     double Lb;
     double Mb;
     double Nb;
     //Expressions variables
     double TEz;
     double Taz;
     double Zxtvcly;
     double Zxtvclz;
     double Zxtvcry;
     double Zxtvcrz;
     //Port Ptvcly pointer
     double *mpND_tortvcly;
     double *mpND_thetatvcly;
     double *mpND_wtvcly;
     double *mpND_ctvcly;
     double *mpND_Zctvcly;
     double *mpND_eqInertiatvcly;
     //Port Ptvclz pointer
     double *mpND_tortvclz;
     double *mpND_thetatvclz;
     double *mpND_wtvclz;
     double *mpND_ctvclz;
     double *mpND_Zctvclz;
     double *mpND_eqInertiatvclz;
     //Port Ptvcry pointer
     double *mpND_tortvcry;
     double *mpND_thetatvcry;
     double *mpND_wtvcry;
     double *mpND_ctvcry;
     double *mpND_Zctvcry;
     double *mpND_eqInertiatvcry;
     //Port Ptvcrz pointer
     double *mpND_tortvcrz;
     double *mpND_thetatvcrz;
     double *mpND_wtvcrz;
     double *mpND_ctvcrz;
     double *mpND_Zctvcrz;
     double *mpND_eqInertiatvcrz;
     //Delay declarations
//==This code has been autogenerated using Compgen==
     //inputVariables pointers
     double *mpthrustl;
     double *mpthrustr;
     double *mpg0;
     double *mpMfuel;
     double *mpMcargo;
     double *mprho;
     double *mpvturbx;
     double *mpvturby;
     double *mpvturbz;
     double *mpwturbx;
     double *mpwturby;
     double *mpwturbz;
     //inputParameters pointers
     double *mpCd0b;
     double *mpCtvc;
     double *mpCLalphabh;
     double *mpCLalphabv;
     double *mphthrust0;
     double *mpIx;
     double *mpIxz;
     double *mpIy;
     double *mpIz;
     double *mpMe;
     double *mpSbh;
     double *mpSbv;
     double *mpxbach;
     double *mpxbacv;
     double *mpxbcge;
     double *mpxcargo;
     double *mpxfuel;
     double *mpxeng;
     double *mpyeng;
     double *mpkground;
     double *mpcground;
     //outputVariables pointers
     double *mpxcg;
     double *mpycg;
     double *mpzcg;
     double *mpvx;
     double *mpvy;
     double *mpvz;
     double *mpPsi;
     double *mpThetao;
     double *mpPhi;
     double *mpUb;
     double *mpVb;
     double *mpWb;
     double *mpPb;
     double *mpQb;
     double *mpRb;
     double *mpq0;
     double *mpq1;
     double *mpq2;
     double *mpq3;
     double *mpAlphaAttack;
     double *mpBetaSlip;
     double *mpaltitude;
     double *mpgfx;
     double *mpgfy;
     double *mpgfz;
     double *mpCL1;
     double *mpCd1;
     double *mpFax;
     double *mpFaz;
     Delay mDelayedPart10;
     Delay mDelayedPart11;
     Delay mDelayedPart20;
     Delay mDelayedPart21;
     Delay mDelayedPart30;
     Delay mDelayedPart31;
     Delay mDelayedPart40;
     Delay mDelayedPart41;
     Delay mDelayedPart50;
     Delay mDelayedPart51;
     Delay mDelayedPart60;
     Delay mDelayedPart61;
     Delay mDelayedPart70;
     Delay mDelayedPart71;
     Delay mDelayedPart80;
     Delay mDelayedPart81;
     Delay mDelayedPart90;
     Delay mDelayedPart91;
     Delay mDelayedPart100;
     Delay mDelayedPart101;
     Delay mDelayedPart110;
     Delay mDelayedPart111;
     Delay mDelayedPart120;
     Delay mDelayedPart121;
     Delay mDelayedPart130;
     Delay mDelayedPart131;
     EquationSystemSolver *mpSolver;

public:
     static Component *Creator()
     {
        return new AeroVehicleTVC2();
     }

     void configure()
     {
//==This code has been autogenerated using Compgen==

        mNstep=9;
        jacobianMatrix.create(13,13);
        systemEquations.create(13);
        delayedPart.create(14,6);
        mNoiter=2;
        jsyseqnweight[0]=1;
        jsyseqnweight[1]=0.67;
        jsyseqnweight[2]=0.5;
        jsyseqnweight[3]=0.5;


        //Add ports to the component
        mpPtvcly=addPowerPort("Ptvcly","NodeMechanicRotational");
        mpPtvclz=addPowerPort("Ptvclz","NodeMechanicRotational");
        mpPtvcry=addPowerPort("Ptvcry","NodeMechanicRotational");
        mpPtvcrz=addPowerPort("Ptvcrz","NodeMechanicRotational");
        //Add inputVariables to the component
            addInputVariable("thrustl","Engine thrust","N",0.,&mpthrustl);
            addInputVariable("thrustr","Engine thrust","N",0.,&mpthrustr);
            addInputVariable("g0","Gravity acceleration","m/s^2",9.81,&mpg0);
            addInputVariable("Mfuel","Fuel weight","kg",0.,&mpMfuel);
            addInputVariable("Mcargo","Cargo weight","kg",0.,&mpMcargo);
            addInputVariable("rho","Air density","kg/m3",1.25,&mprho);
            addInputVariable("vturbx","air turbulence x","m/s",0.,&mpvturbx);
            addInputVariable("vturby","air turbulence y","m/s",0.,&mpvturby);
            addInputVariable("vturbz","air turbulence z","m/s",0.,&mpvturbz);
            addInputVariable("wturbx","air turbulence \
x","rad/s",0.,&mpwturbx);
            addInputVariable("wturby","air turbulence \
y","rad/s",0.,&mpwturby);
            addInputVariable("wturbz","air turbulence \
z","rad/s",0.,&mpwturbz);

        //Add inputParammeters to the component
            addInputVariable("Cd0b", "&mpDrag coef. body", "", \
0.004,&mpCd0b);
            addInputVariable("Ctvc", "&mphinge ciefficient TVC", "m/rad", \
0.1,&mpCtvc);
            addInputVariable("CLalphabh", "&mpL. slope c. body h", "", \
2.,&mpCLalphabh);
            addInputVariable("CLalphabv", "&mpL. slope c. body v", "", \
2.,&mpCLalphabv);
            addInputVariable("hthrust0", "&mpengine vert. pos", "", \
0.,&mphthrust0);
            addInputVariable("Ix", "&mpInertia moment", "kgm2", 1000.,&mpIx);
            addInputVariable("Ixz", "&mpInertia moment", "kgm2", \
500.,&mpIxz);
            addInputVariable("Iy", "&mpInertia moment", "kgm2", 3000.,&mpIy);
            addInputVariable("Iz", "&mpInertia moment", "kgm2", 3000.,&mpIz);
            addInputVariable("Me", "&mpEmpty weight", "kg", 5000.,&mpMe);
            addInputVariable("Sbh", "&mpnorm. hor. proj. area", "", \
5,&mpSbh);
            addInputVariable("Sbv", "&mpnorm.body vert. proj. area", "", \
5,&mpSbv);
            addInputVariable("xbach", "&mpbody ac. hor.", "m", 8.5,&mpxbach);
            addInputVariable("xbacv", "&mpbody ac vert.", "m", 8.5,&mpxbacv);
            addInputVariable("xbcge", "&mpbody cg", "m", 6.,&mpxbcge);
            addInputVariable("xcargo", "&mpcargo pos.", "m", 6.,&mpxcargo);
            addInputVariable("xfuel", "&mp", "m", 6.,&mpxfuel);
            addInputVariable("xeng", "&mpengine thrust centre position", "m", \
0.,&mpxeng);
            addInputVariable("yeng", "&mpengines off. from center", "m", \
0.,&mpyeng);
            addInputVariable("kground", "&mpGround stiffness (for \
limitiation)", "N/m", 10000.,&mpkground);
            addInputVariable("cground", "&mpGround damping (for \
limitiation)", "Ns/m", 1000.,&mpcground);
        //Add outputVariables to the component
            addOutputVariable("xcg","Horizontal position 1","m",0,&mpxcg);
            addOutputVariable("ycg","Horizontal position 2","m",0,&mpycg);
            addOutputVariable("zcg","Vertical position","m",0,&mpzcg);
            addOutputVariable("vx","Horizontal speed 1","m",0,&mpvx);
            addOutputVariable("vy","Horizontal speed 2","m",0,&mpvy);
            addOutputVariable("vz","Vertical speed","m",0,&mpvz);
            addOutputVariable("Psi","Azimuth angle","rad",0,&mpPsi);
            addOutputVariable("Thetao","Elevation angle","rad",0,&mpThetao);
            addOutputVariable("Phi","Bank angle","rad",0,&mpPhi);
            addOutputVariable("Ub","Speed xb-axis","m/s",100,&mpUb);
            addOutputVariable("Vb","Speed yb-axis","m/s",0,&mpVb);
            addOutputVariable("Wb","Speed zb-axis","m/s",0,&mpWb);
            addOutputVariable("Pb","Angular velocity","rad/s",0,&mpPb);
            addOutputVariable("Qb","Angular velocity","rad/s",0,&mpQb);
            addOutputVariable("Rb","Angular velocity","rad/s",0,&mpRb);
            addOutputVariable("q0","quartenion 0","",0,&mpq0);
            addOutputVariable("q1","quartenion 1","",0,&mpq1);
            addOutputVariable("q2","quartenion 2","",0,&mpq2);
            addOutputVariable("q3","quartenion 3","",0,&mpq3);
            addOutputVariable("AlphaAttack","Angle of \
atack","rad",0,&mpAlphaAttack);
            addOutputVariable("BetaSlip","Sideslip \
angle","rad/s",0,&mpBetaSlip);
            addOutputVariable("altitude","altitude","m",0,&mpaltitude);
            addOutputVariable("gfx","g-force in x","m/s^2",0,&mpgfx);
            addOutputVariable("gfy","g-force in y","m/s^2",0,&mpgfy);
            addOutputVariable("gfz","g-force in z","m/s^2",0,&mpgfz);
            addOutputVariable("CL1","Lift coeff. wing 1","",0,&mpCL1);
            addOutputVariable("Cd1","Drag coeff. wing 1","",0,&mpCd1);
            addOutputVariable("Fax","{Fax, 0, double, Aero force in \
z}[[5]]","Aero force in z",0,&mpFax);
            addOutputVariable("Faz","{Faz, 0, double, Aero force in \
x}[[5]]","Aero force in x",0,&mpFaz);

//==This code has been autogenerated using Compgen==
        //Add constantParameters
        mpSolver = new EquationSystemSolver(this,13);
     }

    void initialize()
     {
        //Read port variable pointers from nodes
        //Port Ptvcly
        mpND_tortvcly=getSafeNodeDataPtr(mpPtvcly, \
NodeMechanicRotational::Torque);
        mpND_thetatvcly=getSafeNodeDataPtr(mpPtvcly, \
NodeMechanicRotational::Angle);
        mpND_wtvcly=getSafeNodeDataPtr(mpPtvcly, \
NodeMechanicRotational::AngularVelocity);
        mpND_ctvcly=getSafeNodeDataPtr(mpPtvcly, \
NodeMechanicRotational::WaveVariable);
        mpND_Zctvcly=getSafeNodeDataPtr(mpPtvcly, \
NodeMechanicRotational::CharImpedance);
        mpND_eqInertiatvcly=getSafeNodeDataPtr(mpPtvcly, \
NodeMechanicRotational::EquivalentInertia);
        //Port Ptvclz
        mpND_tortvclz=getSafeNodeDataPtr(mpPtvclz, \
NodeMechanicRotational::Torque);
        mpND_thetatvclz=getSafeNodeDataPtr(mpPtvclz, \
NodeMechanicRotational::Angle);
        mpND_wtvclz=getSafeNodeDataPtr(mpPtvclz, \
NodeMechanicRotational::AngularVelocity);
        mpND_ctvclz=getSafeNodeDataPtr(mpPtvclz, \
NodeMechanicRotational::WaveVariable);
        mpND_Zctvclz=getSafeNodeDataPtr(mpPtvclz, \
NodeMechanicRotational::CharImpedance);
        mpND_eqInertiatvclz=getSafeNodeDataPtr(mpPtvclz, \
NodeMechanicRotational::EquivalentInertia);
        //Port Ptvcry
        mpND_tortvcry=getSafeNodeDataPtr(mpPtvcry, \
NodeMechanicRotational::Torque);
        mpND_thetatvcry=getSafeNodeDataPtr(mpPtvcry, \
NodeMechanicRotational::Angle);
        mpND_wtvcry=getSafeNodeDataPtr(mpPtvcry, \
NodeMechanicRotational::AngularVelocity);
        mpND_ctvcry=getSafeNodeDataPtr(mpPtvcry, \
NodeMechanicRotational::WaveVariable);
        mpND_Zctvcry=getSafeNodeDataPtr(mpPtvcry, \
NodeMechanicRotational::CharImpedance);
        mpND_eqInertiatvcry=getSafeNodeDataPtr(mpPtvcry, \
NodeMechanicRotational::EquivalentInertia);
        //Port Ptvcrz
        mpND_tortvcrz=getSafeNodeDataPtr(mpPtvcrz, \
NodeMechanicRotational::Torque);
        mpND_thetatvcrz=getSafeNodeDataPtr(mpPtvcrz, \
NodeMechanicRotational::Angle);
        mpND_wtvcrz=getSafeNodeDataPtr(mpPtvcrz, \
NodeMechanicRotational::AngularVelocity);
        mpND_ctvcrz=getSafeNodeDataPtr(mpPtvcrz, \
NodeMechanicRotational::WaveVariable);
        mpND_Zctvcrz=getSafeNodeDataPtr(mpPtvcrz, \
NodeMechanicRotational::CharImpedance);
        mpND_eqInertiatvcrz=getSafeNodeDataPtr(mpPtvcrz, \
NodeMechanicRotational::EquivalentInertia);

        //Read variables from nodes
        //Port Ptvcly
        tortvcly = (*mpND_tortvcly);
        thetatvcly = (*mpND_thetatvcly);
        wtvcly = (*mpND_wtvcly);
        ctvcly = (*mpND_ctvcly);
        Zctvcly = (*mpND_Zctvcly);
        eqInertiatvcly = (*mpND_eqInertiatvcly);
        //Port Ptvclz
        tortvclz = (*mpND_tortvclz);
        thetatvclz = (*mpND_thetatvclz);
        wtvclz = (*mpND_wtvclz);
        ctvclz = (*mpND_ctvclz);
        Zctvclz = (*mpND_Zctvclz);
        eqInertiatvclz = (*mpND_eqInertiatvclz);
        //Port Ptvcry
        tortvcry = (*mpND_tortvcry);
        thetatvcry = (*mpND_thetatvcry);
        wtvcry = (*mpND_wtvcry);
        ctvcry = (*mpND_ctvcry);
        Zctvcry = (*mpND_Zctvcry);
        eqInertiatvcry = (*mpND_eqInertiatvcry);
        //Port Ptvcrz
        tortvcrz = (*mpND_tortvcrz);
        thetatvcrz = (*mpND_thetatvcrz);
        wtvcrz = (*mpND_wtvcrz);
        ctvcrz = (*mpND_ctvcrz);
        Zctvcrz = (*mpND_Zctvcrz);
        eqInertiatvcrz = (*mpND_eqInertiatvcrz);

        //Read inputVariables from nodes
        thrustl = (*mpthrustl);
        thrustr = (*mpthrustr);
        g0 = (*mpg0);
        Mfuel = (*mpMfuel);
        Mcargo = (*mpMcargo);
        rho = (*mprho);
        vturbx = (*mpvturbx);
        vturby = (*mpvturby);
        vturbz = (*mpvturbz);
        wturbx = (*mpwturbx);
        wturby = (*mpwturby);
        wturbz = (*mpwturbz);

        //Read inputParameters from nodes
        Cd0b = (*mpCd0b);
        Ctvc = (*mpCtvc);
        CLalphabh = (*mpCLalphabh);
        CLalphabv = (*mpCLalphabv);
        hthrust0 = (*mphthrust0);
        Ix = (*mpIx);
        Ixz = (*mpIxz);
        Iy = (*mpIy);
        Iz = (*mpIz);
        Me = (*mpMe);
        Sbh = (*mpSbh);
        Sbv = (*mpSbv);
        xbach = (*mpxbach);
        xbacv = (*mpxbacv);
        xbcge = (*mpxbcge);
        xcargo = (*mpxcargo);
        xfuel = (*mpxfuel);
        xeng = (*mpxeng);
        yeng = (*mpyeng);
        kground = (*mpkground);
        cground = (*mpcground);

        //Read outputVariables from nodes
        xcg = (*mpxcg);
        ycg = (*mpycg);
        zcg = (*mpzcg);
        vx = (*mpvx);
        vy = (*mpvy);
        vz = (*mpvz);
        Psi = (*mpPsi);
        Thetao = (*mpThetao);
        Phi = (*mpPhi);
        Ub = (*mpUb);
        Vb = (*mpVb);
        Wb = (*mpWb);
        Pb = (*mpPb);
        Qb = (*mpQb);
        Rb = (*mpRb);
        q0 = (*mpq0);
        q1 = (*mpq1);
        q2 = (*mpq2);
        q3 = (*mpq3);
        AlphaAttack = (*mpAlphaAttack);
        BetaSlip = (*mpBetaSlip);
        altitude = (*mpaltitude);
        gfx = (*mpgfx);
        gfy = (*mpgfy);
        gfz = (*mpgfz);
        CL1 = (*mpCL1);
        Cd1 = (*mpCd1);
        Fax = (*mpFax);
        Faz = (*mpFaz);

//==This code has been autogenerated using Compgen==
        //InitialExpressions
        q0 = Cos(Phi/2.)*Cos(Psi/2.)*Cos(Thetao/2.) + \
Sin(Phi/2.)*Sin(Psi/2.)*Sin(Thetao/2.);
        q1 = Cos(Psi/2.)*Cos(Thetao/2.)*Sin(Phi/2.) - \
Cos(Phi/2.)*Sin(Psi/2.)*Sin(Thetao/2.);
        q2 = Cos(Thetao/2.)*Sin(Phi/2.)*Sin(Psi/2.) + \
Cos(Phi/2.)*Cos(Psi/2.)*Sin(Thetao/2.);
        q3 = Cos(Phi/2.)*Cos(Thetao/2.)*Sin(Psi/2.) - \
Cos(Psi/2.)*Sin(Phi/2.)*Sin(Thetao/2.);

        //LocalExpressions
        v = Sqrt(0.0001 + Power(Ub + vturbx,2) + Power(Vb + vturby,2) + \
Power(vturbz + Wb,2));
        Alpha = Atan2L(vturbz + Wb,0.0001 + Ub + vturbx);
        qpress = (rho*Power(v,2))/2.;
        Beta = Atan2L(Vb + vturby,Sqrt(0.0001 + Power(Ub + vturbx,2) + \
Power(vturbz + Wb,2)));
        mass = Mcargo + Me + Mfuel;
        xbcg = (Me*xbcge + Mcargo*xcargo + Mfuel*xfuel)/mass;
        Dragb = qpress*Sbh*(Cd0b + CLalphabh*Power(Sin(Alpha),2) + \
CLalphabv*Power(Sin(Beta),2));
        Mdvtheta = (CLalphabh*qpress*Sbh*Power(xbach - xbcg,2))/(0.1 + v);
        Mdvpsi = (CLalphabv*qpress*Sbv*Power(xbacv - xbcg,2))/(0.1 + v);
        Fx = -(Cd0b*qpress*Sbh*Cos(Alpha)*Cos(Beta)) + \
thrustl*Cos(thetatvcly)*Cos(thetatvclz) + \
thrustr*Cos(thetatvcry)*Cos(thetatvcrz);
        Fy = CLalphabv*qpress*Sbv*Sin(Beta) - \
thrustl*Cos(thetatvclz)*Sin(thetatvcly) - \
thrustr*Cos(thetatvcrz)*Sin(thetatvcry);
        Fz = -(CLalphabh*qpress*Sbh*Sin(Alpha)) - \
thrustl*Cos(thetatvcly)*Sin(thetatvclz) - \
thrustr*Cos(thetatvcry)*Sin(thetatvcrz);
        Lb = yeng*(thrustl*Cos(thetatvcly)*Sin(thetatvclz) - \
thrustr*Cos(thetatvcry)*Sin(thetatvcrz));
        Mb = -(Mdvtheta*(Qb + wturby)) - CLalphabh*qpress*Sbh*(xbach - \
xbcg)*Sin(Alpha) + (xbcg - xeng)*(thrustl*Cos(thetatvcly)*Sin(thetatvclz) + \
thrustr*Cos(thetatvcry)*Sin(thetatvcrz));
        Nb = -(Mdvpsi*(Rb + wturbz)) + CLalphabv*qpress*Sbv*(xbacv - \
xbcg)*Sin(Beta) + (xbcg - xeng)*(thrustl*Cos(thetatvclz)*Sin(thetatvcly) + \
thrustr*Cos(thetatvcrz)*Sin(thetatvcry));

        //Initialize delays
        delayParts1[1] = (-(Fx*mTimestep) + 2*g0*mass*mTimestep*q0*q2 - \
2*g0*mass*mTimestep*q1*q3 - 2*mass*Ub - mass*mTimestep*Rb*Vb + \
mass*mTimestep*Qb*Wb - 2*kground*mTimestep*q0*q2*zcg*onPositive(zcg) + \
2*kground*mTimestep*q1*q3*zcg*onPositive(zcg))/(2.*mass);
        mDelayedPart11.initialize(mNstep,delayParts1[1]);
        delayParts2[1] = (-(Fy*mTimestep) - 2*g0*mass*mTimestep*q0*q1 - \
2*g0*mass*mTimestep*q2*q3 + mass*mTimestep*Rb*Ub - 2*mass*Vb - \
mass*mTimestep*Pb*Wb + 2*kground*mTimestep*q0*q1*zcg*onPositive(zcg) + \
2*kground*mTimestep*q2*q3*zcg*onPositive(zcg))/(2.*mass);
        mDelayedPart21.initialize(mNstep,delayParts2[1]);
        delayParts3[1] = (-(Fz*mTimestep) - g0*mass*mTimestep*Power(q0,2) + \
g0*mass*mTimestep*Power(q1,2) + g0*mass*mTimestep*Power(q2,2) - \
g0*mass*mTimestep*Power(q3,2) - mass*mTimestep*Qb*Ub + mass*mTimestep*Pb*Vb - \
2*mass*Wb + kground*mTimestep*Power(q0,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q1,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q2,2)*zcg*onPositive(zcg) + \
kground*mTimestep*Power(q3,2)*zcg*onPositive(zcg))/(2.*mass);
        mDelayedPart31.initialize(mNstep,delayParts3[1]);
        delayParts4[1] = (Iz*Lb*mTimestep + Ixz*mTimestep*Nb - \
2*Power(Ixz,2)*Pb + 2*Ix*Iz*Pb + Ix*Ixz*mTimestep*Pb*Qb - \
Ixz*Iy*mTimestep*Pb*Qb + Ixz*Iz*mTimestep*Pb*Qb - \
Power(Ixz,2)*mTimestep*Qb*Rb + Iy*Iz*mTimestep*Qb*Rb - \
Power(Iz,2)*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz + Ix*Ixz*mTimestep*Qb \
- Ixz*Iy*mTimestep*Qb + Ixz*Iz*mTimestep*Qb);
        mDelayedPart41.initialize(mNstep,delayParts4[1]);
        delayParts5[1] = (-(Mb*mTimestep) + Ixz*mTimestep*Power(Pb,2) - \
2*Iy*Qb + Ix*mTimestep*Pb*Rb - Iz*mTimestep*Pb*Rb - \
Ixz*mTimestep*Power(Rb,2))/(2.*Iy);
        mDelayedPart51.initialize(mNstep,delayParts5[1]);
        delayParts6[1] = (Ixz*Lb*mTimestep + Ix*mTimestep*Nb + \
Power(Ix,2)*mTimestep*Pb*Qb + Power(Ixz,2)*mTimestep*Pb*Qb - \
Ix*Iy*mTimestep*Pb*Qb - 2*Power(Ixz,2)*Rb + 2*Ix*Iz*Rb - \
Ix*Ixz*mTimestep*Qb*Rb + Ixz*Iy*mTimestep*Qb*Rb - \
Ixz*Iz*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz - Ix*Ixz*mTimestep*Qb + \
Ixz*Iy*mTimestep*Qb - Ixz*Iz*mTimestep*Qb);
        mDelayedPart61.initialize(mNstep,delayParts6[1]);
        delayParts7[1] = (-4*q0 + mTimestep*Pb*q1 + mTimestep*q2*Qb + \
mTimestep*q3*Rb)/4.;
        mDelayedPart71.initialize(mNstep,delayParts7[1]);
        delayParts8[1] = (-(mTimestep*Pb*q0) - 4*q1 + mTimestep*q3*Qb - \
mTimestep*q2*Rb)/4.;
        mDelayedPart81.initialize(mNstep,delayParts8[1]);
        delayParts9[1] = (-4*q2 - mTimestep*Pb*q3 - mTimestep*q0*Qb + \
mTimestep*q1*Rb)/4.;
        mDelayedPart91.initialize(mNstep,delayParts9[1]);
        delayParts10[1] = (mTimestep*Pb*q2 - 4*q3 - mTimestep*q1*Qb - \
mTimestep*q0*Rb)/4.;
        mDelayedPart101.initialize(mNstep,delayParts10[1]);
        delayParts11[1] = (-(mTimestep*Power(q0,2)*Ub) - \
mTimestep*Power(q1,2)*Ub + mTimestep*Power(q2,2)*Ub + \
mTimestep*Power(q3,2)*Ub - 2*mTimestep*q1*q2*Vb + 2*mTimestep*q0*q3*Vb - \
2*mTimestep*q0*q2*Wb - 2*mTimestep*q1*q3*Wb - 2*xcg)/2.;
        mDelayedPart111.initialize(mNstep,delayParts11[1]);
        delayParts12[1] = (-2*mTimestep*q1*q2*Ub - 2*mTimestep*q0*q3*Ub - \
mTimestep*Power(q0,2)*Vb + mTimestep*Power(q1,2)*Vb - \
mTimestep*Power(q2,2)*Vb + mTimestep*Power(q3,2)*Vb + 2*mTimestep*q0*q1*Wb - \
2*mTimestep*q2*q3*Wb - 2*ycg)/2.;
        mDelayedPart121.initialize(mNstep,delayParts12[1]);
        delayParts13[1] = (2*mTimestep*q0*q2*Ub - 2*mTimestep*q1*q3*Ub - \
2*mTimestep*q0*q1*Vb - 2*mTimestep*q2*q3*Vb - mTimestep*Power(q0,2)*Wb + \
mTimestep*Power(q1,2)*Wb + mTimestep*Power(q2,2)*Wb - \
mTimestep*Power(q3,2)*Wb - 2*zcg)/2.;
        mDelayedPart131.initialize(mNstep,delayParts13[1]);

        delayedPart[1][1] = delayParts1[1];
        delayedPart[2][1] = delayParts2[1];
        delayedPart[3][1] = delayParts3[1];
        delayedPart[4][1] = delayParts4[1];
        delayedPart[5][1] = delayParts5[1];
        delayedPart[6][1] = delayParts6[1];
        delayedPart[7][1] = delayParts7[1];
        delayedPart[8][1] = delayParts8[1];
        delayedPart[9][1] = delayParts9[1];
        delayedPart[10][1] = delayParts10[1];
        delayedPart[11][1] = delayParts11[1];
        delayedPart[12][1] = delayParts12[1];
        delayedPart[13][1] = delayParts13[1];
     }
    void simulateOneTimestep()
     {
        Vec stateVar(13);
        Vec stateVark(13);
        Vec deltaStateVar(13);

        //Read variables from nodes
        //Port Ptvcly
        tortvcly = (*mpND_tortvcly);
        thetatvcly = (*mpND_thetatvcly);
        wtvcly = (*mpND_wtvcly);
        eqInertiatvcly = (*mpND_eqInertiatvcly);
        //Port Ptvclz
        tortvclz = (*mpND_tortvclz);
        thetatvclz = (*mpND_thetatvclz);
        wtvclz = (*mpND_wtvclz);
        eqInertiatvclz = (*mpND_eqInertiatvclz);
        //Port Ptvcry
        tortvcry = (*mpND_tortvcry);
        thetatvcry = (*mpND_thetatvcry);
        wtvcry = (*mpND_wtvcry);
        eqInertiatvcry = (*mpND_eqInertiatvcry);
        //Port Ptvcrz
        tortvcrz = (*mpND_tortvcrz);
        thetatvcrz = (*mpND_thetatvcrz);
        wtvcrz = (*mpND_wtvcrz);
        eqInertiatvcrz = (*mpND_eqInertiatvcrz);

        //Read inputVariables from nodes
        thrustl = (*mpthrustl);
        thrustr = (*mpthrustr);
        g0 = (*mpg0);
        Mfuel = (*mpMfuel);
        Mcargo = (*mpMcargo);
        rho = (*mprho);
        vturbx = (*mpvturbx);
        vturby = (*mpvturby);
        vturbz = (*mpvturbz);
        wturbx = (*mpwturbx);
        wturby = (*mpwturby);
        wturbz = (*mpwturbz);

        //LocalExpressions
        v = Sqrt(0.0001 + Power(Ub + vturbx,2) + Power(Vb + vturby,2) + \
Power(vturbz + Wb,2));
        Alpha = Atan2L(vturbz + Wb,0.0001 + Ub + vturbx);
        qpress = (rho*Power(v,2))/2.;
        Beta = Atan2L(Vb + vturby,Sqrt(0.0001 + Power(Ub + vturbx,2) + \
Power(vturbz + Wb,2)));
        mass = Mcargo + Me + Mfuel;
        xbcg = (Me*xbcge + Mcargo*xcargo + Mfuel*xfuel)/mass;
        Dragb = qpress*Sbh*(Cd0b + CLalphabh*Power(Sin(Alpha),2) + \
CLalphabv*Power(Sin(Beta),2));
        Mdvtheta = (CLalphabh*qpress*Sbh*Power(xbach - xbcg,2))/(0.1 + v);
        Mdvpsi = (CLalphabv*qpress*Sbv*Power(xbacv - xbcg,2))/(0.1 + v);
        Fx = -(Cd0b*qpress*Sbh*Cos(Alpha)*Cos(Beta)) + \
thrustl*Cos(thetatvcly)*Cos(thetatvclz) + \
thrustr*Cos(thetatvcry)*Cos(thetatvcrz);
        Fy = CLalphabv*qpress*Sbv*Sin(Beta) - \
thrustl*Cos(thetatvclz)*Sin(thetatvcly) - \
thrustr*Cos(thetatvcrz)*Sin(thetatvcry);
        Fz = -(CLalphabh*qpress*Sbh*Sin(Alpha)) - \
thrustl*Cos(thetatvcly)*Sin(thetatvclz) - \
thrustr*Cos(thetatvcry)*Sin(thetatvcrz);
        Lb = yeng*(thrustl*Cos(thetatvcly)*Sin(thetatvclz) - \
thrustr*Cos(thetatvcry)*Sin(thetatvcrz));
        Mb = -(Mdvtheta*(Qb + wturby)) - CLalphabh*qpress*Sbh*(xbach - \
xbcg)*Sin(Alpha) + (xbcg - xeng)*(thrustl*Cos(thetatvcly)*Sin(thetatvclz) + \
thrustr*Cos(thetatvcry)*Sin(thetatvcrz));
        Nb = -(Mdvpsi*(Rb + wturbz)) + CLalphabv*qpress*Sbv*(xbacv - \
xbcg)*Sin(Beta) + (xbcg - xeng)*(thrustl*Cos(thetatvclz)*Sin(thetatvcly) + \
thrustr*Cos(thetatvcrz)*Sin(thetatvcry));

        //Initializing variable vector for Newton-Raphson
        stateVark[0] = Ub;
        stateVark[1] = Vb;
        stateVark[2] = Wb;
        stateVark[3] = Pb;
        stateVark[4] = Qb;
        stateVark[5] = Rb;
        stateVark[6] = q0;
        stateVark[7] = q1;
        stateVark[8] = q2;
        stateVark[9] = q3;
        stateVark[10] = xcg;
        stateVark[11] = ycg;
        stateVark[12] = zcg;

        //Iterative solution using Newton-Rapshson
        for(iter=1;iter<=mNoiter;iter++)
        {
         //VehicleTVC2
         //Differential-algebraic system of equation parts

          //Assemble differential-algebraic equations
          systemEquations[0] =Ub - (mTimestep*(Fx + mass*(-2*g0*q0*q2 + \
2*g0*q1*q3 + Rb*Vb - Qb*Wb) + 2*kground*(q0*q2 - \
q1*q3)*zcg*onPositive(zcg)))/(2.*mass) + delayedPart[1][1];
          systemEquations[1] =Vb - (mTimestep*(Fy + mass*(2*g0*q0*q1 + \
2*g0*q2*q3 - Rb*Ub + Pb*Wb) - 2*kground*(q0*q1 + \
q2*q3)*zcg*onPositive(zcg)))/(2.*mass) + delayedPart[2][1];
          systemEquations[2] =Wb - (mTimestep*(Fz + mass*(g0*(Power(q0,2) - \
Power(q1,2) - Power(q2,2) + Power(q3,2)) + Qb*Ub - Pb*Vb) + \
kground*(-Power(q0,2) + Power(q1,2) + Power(q2,2) - \
Power(q3,2))*zcg*onPositive(zcg)))/(2.*mass) + delayedPart[3][1];
          systemEquations[3] =Pb + (mTimestep*(-(Power(Iz,2)*Qb*Rb) + Ixz*(Nb \
- Ixz*Qb*Rb) + Iz*(Lb + Iy*Qb*Rb)))/(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - Iy \
+ Iz)*mTimestep*Qb) + delayedPart[4][1];
          systemEquations[4] =Qb - (mTimestep*(Mb + (-Ix + Iz)*Pb*Rb + \
Ixz*(-Power(Pb,2) + Power(Rb,2))))/(2.*Iy) + delayedPart[5][1];
          systemEquations[5] =-((mTimestep*(Ixz*Lb + Power(Ixz,2)*Pb*Qb + \
Ix*(Nb + (Ix - Iy)*Pb*Qb)))/(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + \
Iz)*mTimestep*Qb)) + Rb + delayedPart[6][1];
          systemEquations[6] =q0 + (mTimestep*(Pb*q1 + q2*Qb + q3*Rb))/4. + \
delayedPart[7][1];
          systemEquations[7] =q1 - (mTimestep*(Pb*q0 - q3*Qb + q2*Rb))/4. + \
delayedPart[8][1];
          systemEquations[8] =q2 - (mTimestep*(Pb*q3 + q0*Qb - q1*Rb))/4. + \
delayedPart[9][1];
          systemEquations[9] =q3 + (mTimestep*(Pb*q2 - q1*Qb - q0*Rb))/4. + \
delayedPart[10][1];
          systemEquations[10] =-(mTimestep*(Power(q0,2)*Ub + Power(q1,2)*Ub - \
(Power(q2,2) + Power(q3,2))*Ub + q0*(-2*q3*Vb + 2*q2*Wb) + 2*q1*(q2*Vb + \
q3*Wb)))/2. + xcg + delayedPart[11][1];
          systemEquations[11] =(mTimestep*(-2*q1*q2*Ub - 2*q0*q3*Ub - \
Power(q0,2)*Vb + Power(q1,2)*Vb - Power(q2,2)*Vb + Power(q3,2)*Vb + \
2*q0*q1*Wb - 2*q2*q3*Wb))/2. + ycg + delayedPart[12][1];
          systemEquations[12] =-(mTimestep*(-2*q0*q2*Ub + 2*q1*q3*Ub + \
2*q0*q1*Vb + 2*q2*q3*Vb + Power(q0,2)*Wb - Power(q1,2)*Wb - Power(q2,2)*Wb + \
Power(q3,2)*Wb))/2. + zcg + delayedPart[13][1];

          //Jacobian matrix
          jacobianMatrix[0][0] = 1;
          jacobianMatrix[0][1] = -(mTimestep*Rb)/2.;
          jacobianMatrix[0][2] = (mTimestep*Qb)/2.;
          jacobianMatrix[0][3] = 0;
          jacobianMatrix[0][4] = (mTimestep*Wb)/2.;
          jacobianMatrix[0][5] = -(mTimestep*Vb)/2.;
          jacobianMatrix[0][6] = -(mTimestep*(-2*g0*mass*q2 + \
2*kground*q2*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][7] = -(mTimestep*(2*g0*mass*q3 - \
2*kground*q3*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][8] = -(mTimestep*(-2*g0*mass*q0 + \
2*kground*q0*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][9] = -(mTimestep*(2*g0*mass*q1 - \
2*kground*q1*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[0][10] = 0;
          jacobianMatrix[0][11] = 0;
          jacobianMatrix[0][12] = -((kground*mTimestep*(q0*q2 - \
q1*q3)*onPositive(zcg))/mass);
          jacobianMatrix[1][0] = (mTimestep*Rb)/2.;
          jacobianMatrix[1][1] = 1;
          jacobianMatrix[1][2] = -(mTimestep*Pb)/2.;
          jacobianMatrix[1][3] = -(mTimestep*Wb)/2.;
          jacobianMatrix[1][4] = 0;
          jacobianMatrix[1][5] = (mTimestep*Ub)/2.;
          jacobianMatrix[1][6] = -(mTimestep*(2*g0*mass*q1 - \
2*kground*q1*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][7] = -(mTimestep*(2*g0*mass*q0 - \
2*kground*q0*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][8] = -(mTimestep*(2*g0*mass*q3 - \
2*kground*q3*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][9] = -(mTimestep*(2*g0*mass*q2 - \
2*kground*q2*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[1][10] = 0;
          jacobianMatrix[1][11] = 0;
          jacobianMatrix[1][12] = (kground*mTimestep*(q0*q1 + \
q2*q3)*onPositive(zcg))/mass;
          jacobianMatrix[2][0] = -(mTimestep*Qb)/2.;
          jacobianMatrix[2][1] = (mTimestep*Pb)/2.;
          jacobianMatrix[2][2] = 1;
          jacobianMatrix[2][3] = (mTimestep*Vb)/2.;
          jacobianMatrix[2][4] = -(mTimestep*Ub)/2.;
          jacobianMatrix[2][5] = 0;
          jacobianMatrix[2][6] = -(mTimestep*(2*g0*mass*q0 - \
2*kground*q0*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][7] = -(mTimestep*(-2*g0*mass*q1 + \
2*kground*q1*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][8] = -(mTimestep*(-2*g0*mass*q2 + \
2*kground*q2*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][9] = -(mTimestep*(2*g0*mass*q3 - \
2*kground*q3*zcg*onPositive(zcg)))/(2.*mass);
          jacobianMatrix[2][10] = 0;
          jacobianMatrix[2][11] = 0;
          jacobianMatrix[2][12] = -(kground*mTimestep*(-Power(q0,2) + \
Power(q1,2) + Power(q2,2) - Power(q3,2))*onPositive(zcg))/(2.*mass);
          jacobianMatrix[3][0] = 0;
          jacobianMatrix[3][1] = 0;
          jacobianMatrix[3][2] = 0;
          jacobianMatrix[3][3] = 1;
          jacobianMatrix[3][4] = (mTimestep*(-(Power(Ixz,2)*Rb) + Iy*Iz*Rb - \
Power(Iz,2)*Rb))/(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - Iy + Iz)*mTimestep*Qb) \
- (Ixz*(Ix - Iy + Iz)*Power(mTimestep,2)*(-(Power(Iz,2)*Qb*Rb) + Ixz*(Nb - \
Ixz*Qb*Rb) + Iz*(Lb + Iy*Qb*Rb)))/Power(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - \
Iy + Iz)*mTimestep*Qb,2);
          jacobianMatrix[3][5] = (mTimestep*(-(Power(Ixz,2)*Qb) + Iy*Iz*Qb - \
Power(Iz,2)*Qb))/(2*Power(Ixz,2) - 2*Ix*Iz + Ixz*(Ix - Iy + \
Iz)*mTimestep*Qb);
          jacobianMatrix[3][6] = 0;
          jacobianMatrix[3][7] = 0;
          jacobianMatrix[3][8] = 0;
          jacobianMatrix[3][9] = 0;
          jacobianMatrix[3][10] = 0;
          jacobianMatrix[3][11] = 0;
          jacobianMatrix[3][12] = 0;
          jacobianMatrix[4][0] = 0;
          jacobianMatrix[4][1] = 0;
          jacobianMatrix[4][2] = 0;
          jacobianMatrix[4][3] = -(mTimestep*(-2*Ixz*Pb + (-Ix + \
Iz)*Rb))/(2.*Iy);
          jacobianMatrix[4][4] = 1;
          jacobianMatrix[4][5] = -(mTimestep*((-Ix + Iz)*Pb + \
2*Ixz*Rb))/(2.*Iy);
          jacobianMatrix[4][6] = 0;
          jacobianMatrix[4][7] = 0;
          jacobianMatrix[4][8] = 0;
          jacobianMatrix[4][9] = 0;
          jacobianMatrix[4][10] = 0;
          jacobianMatrix[4][11] = 0;
          jacobianMatrix[4][12] = 0;
          jacobianMatrix[5][0] = 0;
          jacobianMatrix[5][1] = 0;
          jacobianMatrix[5][2] = 0;
          jacobianMatrix[5][3] = -((mTimestep*(Power(Ixz,2)*Qb + Ix*(Ix - \
Iy)*Qb))/(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + Iz)*mTimestep*Qb));
          jacobianMatrix[5][4] = -((mTimestep*(Power(Ixz,2)*Pb + Ix*(Ix - \
Iy)*Pb))/(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + Iz)*mTimestep*Qb)) + \
(Ixz*(Ix - Iy + Iz)*Power(mTimestep,2)*(Ixz*Lb + Power(Ixz,2)*Pb*Qb + Ix*(Nb \
+ (Ix - Iy)*Pb*Qb)))/Power(-2*Power(Ixz,2) + 2*Ix*Iz + Ixz*(Ix - Iy + \
Iz)*mTimestep*Qb,2);
          jacobianMatrix[5][5] = 1;
          jacobianMatrix[5][6] = 0;
          jacobianMatrix[5][7] = 0;
          jacobianMatrix[5][8] = 0;
          jacobianMatrix[5][9] = 0;
          jacobianMatrix[5][10] = 0;
          jacobianMatrix[5][11] = 0;
          jacobianMatrix[5][12] = 0;
          jacobianMatrix[6][0] = 0;
          jacobianMatrix[6][1] = 0;
          jacobianMatrix[6][2] = 0;
          jacobianMatrix[6][3] = (mTimestep*q1)/4.;
          jacobianMatrix[6][4] = (mTimestep*q2)/4.;
          jacobianMatrix[6][5] = (mTimestep*q3)/4.;
          jacobianMatrix[6][6] = 1;
          jacobianMatrix[6][7] = (mTimestep*Pb)/4.;
          jacobianMatrix[6][8] = (mTimestep*Qb)/4.;
          jacobianMatrix[6][9] = (mTimestep*Rb)/4.;
          jacobianMatrix[6][10] = 0;
          jacobianMatrix[6][11] = 0;
          jacobianMatrix[6][12] = 0;
          jacobianMatrix[7][0] = 0;
          jacobianMatrix[7][1] = 0;
          jacobianMatrix[7][2] = 0;
          jacobianMatrix[7][3] = -(mTimestep*q0)/4.;
          jacobianMatrix[7][4] = (mTimestep*q3)/4.;
          jacobianMatrix[7][5] = -(mTimestep*q2)/4.;
          jacobianMatrix[7][6] = -(mTimestep*Pb)/4.;
          jacobianMatrix[7][7] = 1;
          jacobianMatrix[7][8] = -(mTimestep*Rb)/4.;
          jacobianMatrix[7][9] = (mTimestep*Qb)/4.;
          jacobianMatrix[7][10] = 0;
          jacobianMatrix[7][11] = 0;
          jacobianMatrix[7][12] = 0;
          jacobianMatrix[8][0] = 0;
          jacobianMatrix[8][1] = 0;
          jacobianMatrix[8][2] = 0;
          jacobianMatrix[8][3] = -(mTimestep*q3)/4.;
          jacobianMatrix[8][4] = -(mTimestep*q0)/4.;
          jacobianMatrix[8][5] = (mTimestep*q1)/4.;
          jacobianMatrix[8][6] = -(mTimestep*Qb)/4.;
          jacobianMatrix[8][7] = (mTimestep*Rb)/4.;
          jacobianMatrix[8][8] = 1;
          jacobianMatrix[8][9] = -(mTimestep*Pb)/4.;
          jacobianMatrix[8][10] = 0;
          jacobianMatrix[8][11] = 0;
          jacobianMatrix[8][12] = 0;
          jacobianMatrix[9][0] = 0;
          jacobianMatrix[9][1] = 0;
          jacobianMatrix[9][2] = 0;
          jacobianMatrix[9][3] = (mTimestep*q2)/4.;
          jacobianMatrix[9][4] = -(mTimestep*q1)/4.;
          jacobianMatrix[9][5] = -(mTimestep*q0)/4.;
          jacobianMatrix[9][6] = -(mTimestep*Rb)/4.;
          jacobianMatrix[9][7] = -(mTimestep*Qb)/4.;
          jacobianMatrix[9][8] = (mTimestep*Pb)/4.;
          jacobianMatrix[9][9] = 1;
          jacobianMatrix[9][10] = 0;
          jacobianMatrix[9][11] = 0;
          jacobianMatrix[9][12] = 0;
          jacobianMatrix[10][0] = -(mTimestep*(Power(q0,2) + Power(q1,2) - \
Power(q2,2) - Power(q3,2)))/2.;
          jacobianMatrix[10][1] = -(mTimestep*(2*q1*q2 - 2*q0*q3))/2.;
          jacobianMatrix[10][2] = -(mTimestep*(2*q0*q2 + 2*q1*q3))/2.;
          jacobianMatrix[10][3] = 0;
          jacobianMatrix[10][4] = 0;
          jacobianMatrix[10][5] = 0;
          jacobianMatrix[10][6] = -(mTimestep*(2*q0*Ub - 2*q3*Vb + \
2*q2*Wb))/2.;
          jacobianMatrix[10][7] = -(mTimestep*(2*q1*Ub + 2*(q2*Vb + \
q3*Wb)))/2.;
          jacobianMatrix[10][8] = -(mTimestep*(-2*q2*Ub + 2*q1*Vb + \
2*q0*Wb))/2.;
          jacobianMatrix[10][9] = -(mTimestep*(-2*q3*Ub - 2*q0*Vb + \
2*q1*Wb))/2.;
          jacobianMatrix[10][10] = 1;
          jacobianMatrix[10][11] = 0;
          jacobianMatrix[10][12] = 0;
          jacobianMatrix[11][0] = (mTimestep*(-2*q1*q2 - 2*q0*q3))/2.;
          jacobianMatrix[11][1] = (mTimestep*(-Power(q0,2) + Power(q1,2) - \
Power(q2,2) + Power(q3,2)))/2.;
          jacobianMatrix[11][2] = (mTimestep*(2*q0*q1 - 2*q2*q3))/2.;
          jacobianMatrix[11][3] = 0;
          jacobianMatrix[11][4] = 0;
          jacobianMatrix[11][5] = 0;
          jacobianMatrix[11][6] = (mTimestep*(-2*q3*Ub - 2*q0*Vb + \
2*q1*Wb))/2.;
          jacobianMatrix[11][7] = (mTimestep*(-2*q2*Ub + 2*q1*Vb + \
2*q0*Wb))/2.;
          jacobianMatrix[11][8] = (mTimestep*(-2*q1*Ub - 2*q2*Vb - \
2*q3*Wb))/2.;
          jacobianMatrix[11][9] = (mTimestep*(-2*q0*Ub + 2*q3*Vb - \
2*q2*Wb))/2.;
          jacobianMatrix[11][10] = 0;
          jacobianMatrix[11][11] = 1;
          jacobianMatrix[11][12] = 0;
          jacobianMatrix[12][0] = -(mTimestep*(-2*q0*q2 + 2*q1*q3))/2.;
          jacobianMatrix[12][1] = -(mTimestep*(2*q0*q1 + 2*q2*q3))/2.;
          jacobianMatrix[12][2] = -(mTimestep*(Power(q0,2) - Power(q1,2) - \
Power(q2,2) + Power(q3,2)))/2.;
          jacobianMatrix[12][3] = 0;
          jacobianMatrix[12][4] = 0;
          jacobianMatrix[12][5] = 0;
          jacobianMatrix[12][6] = -(mTimestep*(-2*q2*Ub + 2*q1*Vb + \
2*q0*Wb))/2.;
          jacobianMatrix[12][7] = -(mTimestep*(2*q3*Ub + 2*q0*Vb - \
2*q1*Wb))/2.;
          jacobianMatrix[12][8] = -(mTimestep*(-2*q0*Ub + 2*q3*Vb - \
2*q2*Wb))/2.;
          jacobianMatrix[12][9] = -(mTimestep*(2*q1*Ub + 2*q2*Vb + \
2*q3*Wb))/2.;
          jacobianMatrix[12][10] = 0;
          jacobianMatrix[12][11] = 0;
          jacobianMatrix[12][12] = 1;
//==This code has been autogenerated using Compgen==

          //Solving equation using LU-faktorisation
          mpSolver->solve(jacobianMatrix, systemEquations, stateVark, iter);
          Ub=stateVark[0];
          Vb=stateVark[1];
          Wb=stateVark[2];
          Pb=stateVark[3];
          Qb=stateVark[4];
          Rb=stateVark[5];
          q0=stateVark[6];
          q1=stateVark[7];
          q2=stateVark[8];
          q3=stateVark[9];
          xcg=stateVark[10];
          ycg=stateVark[11];
          zcg=stateVark[12];
          //Expressions
          vx = (Power(q0,2) + Power(q1,2) - Power(q2,2) - Power(q3,2))*Ub + \
2*(q1*q2 - q0*q3)*Vb + 2*(q0*q2 + q1*q3)*Wb;
          vy = 2*(q1*q2 + q0*q3)*Ub + (Power(q0,2) - Power(q1,2) + \
Power(q2,2) - Power(q3,2))*Vb + 2*(-(q0*q1) + q2*q3)*Wb;
          vz = 2*(-(q0*q2) + q1*q3)*Ub + 2*(q0*q1 + q2*q3)*Vb + (Power(q0,2) \
- Power(q1,2) - Power(q2,2) + Power(q3,2))*Wb;
          AlphaAttack = Alpha;
          BetaSlip = Beta;
          altitude = -zcg;
          Phi = Atan2L(2*(q0*q1 + q2*q3),Power(q0,2) - Power(q1,2) - \
Power(q2,2) + Power(q3,2));
          Thetao = ArcSinL(2*(q0*q2 - q1*q3));
          Psi = Atan2L(2*(q1*q2 + q0*q3),Power(q0,2) + Power(q1,2) - \
Power(q2,2) - Power(q3,2));
          gfx = Fx/mass;
          gfy = Fy/mass;
          gfz = Fz/mass;
          Faz = -1;
          Fax = -1;
          TEz = xbcg - xeng;
          Taz = -(Mdvpsi*(Rb + wturbz));
          Zxtvcly = Ctvc*mTimestep*thrustl;
          Zxtvclz = Ctvc*mTimestep*thrustl;
          Zxtvcry = Ctvc*mTimestep*thrustl;
          Zxtvcrz = Ctvc*mTimestep*thrustl;
        }

        //Calculate the delayed parts
        delayParts1[1] = (-(Fx*mTimestep) + 2*g0*mass*mTimestep*q0*q2 - \
2*g0*mass*mTimestep*q1*q3 - 2*mass*Ub - mass*mTimestep*Rb*Vb + \
mass*mTimestep*Qb*Wb - 2*kground*mTimestep*q0*q2*zcg*onPositive(zcg) + \
2*kground*mTimestep*q1*q3*zcg*onPositive(zcg))/(2.*mass);
        delayParts2[1] = (-(Fy*mTimestep) - 2*g0*mass*mTimestep*q0*q1 - \
2*g0*mass*mTimestep*q2*q3 + mass*mTimestep*Rb*Ub - 2*mass*Vb - \
mass*mTimestep*Pb*Wb + 2*kground*mTimestep*q0*q1*zcg*onPositive(zcg) + \
2*kground*mTimestep*q2*q3*zcg*onPositive(zcg))/(2.*mass);
        delayParts3[1] = (-(Fz*mTimestep) - g0*mass*mTimestep*Power(q0,2) + \
g0*mass*mTimestep*Power(q1,2) + g0*mass*mTimestep*Power(q2,2) - \
g0*mass*mTimestep*Power(q3,2) - mass*mTimestep*Qb*Ub + mass*mTimestep*Pb*Vb - \
2*mass*Wb + kground*mTimestep*Power(q0,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q1,2)*zcg*onPositive(zcg) - \
kground*mTimestep*Power(q2,2)*zcg*onPositive(zcg) + \
kground*mTimestep*Power(q3,2)*zcg*onPositive(zcg))/(2.*mass);
        delayParts4[1] = (Iz*Lb*mTimestep + Ixz*mTimestep*Nb - \
2*Power(Ixz,2)*Pb + 2*Ix*Iz*Pb + Ix*Ixz*mTimestep*Pb*Qb - \
Ixz*Iy*mTimestep*Pb*Qb + Ixz*Iz*mTimestep*Pb*Qb - \
Power(Ixz,2)*mTimestep*Qb*Rb + Iy*Iz*mTimestep*Qb*Rb - \
Power(Iz,2)*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz + Ix*Ixz*mTimestep*Qb \
- Ixz*Iy*mTimestep*Qb + Ixz*Iz*mTimestep*Qb);
        delayParts5[1] = (-(Mb*mTimestep) + Ixz*mTimestep*Power(Pb,2) - \
2*Iy*Qb + Ix*mTimestep*Pb*Rb - Iz*mTimestep*Pb*Rb - \
Ixz*mTimestep*Power(Rb,2))/(2.*Iy);
        delayParts6[1] = (Ixz*Lb*mTimestep + Ix*mTimestep*Nb + \
Power(Ix,2)*mTimestep*Pb*Qb + Power(Ixz,2)*mTimestep*Pb*Qb - \
Ix*Iy*mTimestep*Pb*Qb - 2*Power(Ixz,2)*Rb + 2*Ix*Iz*Rb - \
Ix*Ixz*mTimestep*Qb*Rb + Ixz*Iy*mTimestep*Qb*Rb - \
Ixz*Iz*mTimestep*Qb*Rb)/(2*Power(Ixz,2) - 2*Ix*Iz - Ix*Ixz*mTimestep*Qb + \
Ixz*Iy*mTimestep*Qb - Ixz*Iz*mTimestep*Qb);
        delayParts7[1] = (-4*q0 + mTimestep*Pb*q1 + mTimestep*q2*Qb + \
mTimestep*q3*Rb)/4.;
        delayParts8[1] = (-(mTimestep*Pb*q0) - 4*q1 + mTimestep*q3*Qb - \
mTimestep*q2*Rb)/4.;
        delayParts9[1] = (-4*q2 - mTimestep*Pb*q3 - mTimestep*q0*Qb + \
mTimestep*q1*Rb)/4.;
        delayParts10[1] = (mTimestep*Pb*q2 - 4*q3 - mTimestep*q1*Qb - \
mTimestep*q0*Rb)/4.;
        delayParts11[1] = (-(mTimestep*Power(q0,2)*Ub) - \
mTimestep*Power(q1,2)*Ub + mTimestep*Power(q2,2)*Ub + \
mTimestep*Power(q3,2)*Ub - 2*mTimestep*q1*q2*Vb + 2*mTimestep*q0*q3*Vb - \
2*mTimestep*q0*q2*Wb - 2*mTimestep*q1*q3*Wb - 2*xcg)/2.;
        delayParts12[1] = (-2*mTimestep*q1*q2*Ub - 2*mTimestep*q0*q3*Ub - \
mTimestep*Power(q0,2)*Vb + mTimestep*Power(q1,2)*Vb - \
mTimestep*Power(q2,2)*Vb + mTimestep*Power(q3,2)*Vb + 2*mTimestep*q0*q1*Wb - \
2*mTimestep*q2*q3*Wb - 2*ycg)/2.;
        delayParts13[1] = (2*mTimestep*q0*q2*Ub - 2*mTimestep*q1*q3*Ub - \
2*mTimestep*q0*q1*Vb - 2*mTimestep*q2*q3*Vb - mTimestep*Power(q0,2)*Wb + \
mTimestep*Power(q1,2)*Wb + mTimestep*Power(q2,2)*Wb - \
mTimestep*Power(q3,2)*Wb - 2*zcg)/2.;

        delayedPart[1][1] = delayParts1[1];
        delayedPart[2][1] = delayParts2[1];
        delayedPart[3][1] = delayParts3[1];
        delayedPart[4][1] = delayParts4[1];
        delayedPart[5][1] = delayParts5[1];
        delayedPart[6][1] = delayParts6[1];
        delayedPart[7][1] = delayParts7[1];
        delayedPart[8][1] = delayParts8[1];
        delayedPart[9][1] = delayParts9[1];
        delayedPart[10][1] = delayParts10[1];
        delayedPart[11][1] = delayParts11[1];
        delayedPart[12][1] = delayParts12[1];
        delayedPart[13][1] = delayParts13[1];

        //Write new values to nodes
        //Port Ptvcly
        (*mpND_ctvcly)=ctvcly;
        (*mpND_Zctvcly)=Zctvcly;
        //Port Ptvclz
        (*mpND_ctvclz)=ctvclz;
        (*mpND_Zctvclz)=Zctvclz;
        //Port Ptvcry
        (*mpND_ctvcry)=ctvcry;
        (*mpND_Zctvcry)=Zctvcry;
        //Port Ptvcrz
        (*mpND_ctvcrz)=ctvcrz;
        (*mpND_Zctvcrz)=Zctvcrz;
        //outputVariables
        (*mpxcg)=xcg;
        (*mpycg)=ycg;
        (*mpzcg)=zcg;
        (*mpvx)=vx;
        (*mpvy)=vy;
        (*mpvz)=vz;
        (*mpPsi)=Psi;
        (*mpThetao)=Thetao;
        (*mpPhi)=Phi;
        (*mpUb)=Ub;
        (*mpVb)=Vb;
        (*mpWb)=Wb;
        (*mpPb)=Pb;
        (*mpQb)=Qb;
        (*mpRb)=Rb;
        (*mpq0)=q0;
        (*mpq1)=q1;
        (*mpq2)=q2;
        (*mpq3)=q3;
        (*mpAlphaAttack)=AlphaAttack;
        (*mpBetaSlip)=BetaSlip;
        (*mpaltitude)=altitude;
        (*mpgfx)=gfx;
        (*mpgfy)=gfy;
        (*mpgfz)=gfz;
        (*mpCL1)=CL1;
        (*mpCd1)=Cd1;
        (*mpFax)=Fax;
        (*mpFaz)=Faz;

        //Update the delayed variabels
        mDelayedPart11.update(delayParts1[1]);
        mDelayedPart21.update(delayParts2[1]);
        mDelayedPart31.update(delayParts3[1]);
        mDelayedPart41.update(delayParts4[1]);
        mDelayedPart51.update(delayParts5[1]);
        mDelayedPart61.update(delayParts6[1]);
        mDelayedPart71.update(delayParts7[1]);
        mDelayedPart81.update(delayParts8[1]);
        mDelayedPart91.update(delayParts9[1]);
        mDelayedPart101.update(delayParts10[1]);
        mDelayedPart111.update(delayParts11[1]);
        mDelayedPart121.update(delayParts12[1]);
        mDelayedPart131.update(delayParts13[1]);

     }
    void deconfigure()
    {
        delete mpSolver;
    }
};
#endif // AEROVEHICLETVC2_HPP_INCLUDED
